#include "../include/featureObjectExtractor.h"
#include "../include/LSfeatureObject.h"
#include "stdlib.h"
featureObjectExtractor::featureObjectExtractor()
{

}
vector <LSfeatureObject> featureObjectExtractor::doExtract(vector<objectPoint> p, double MaxLength , double minLength)
{
    vector <LSfeatureObject>  lst;
    int startObjIndex, s = 0, i = 0;
    LSfeatureObject  *sObj;
    LSfeatureObject lastObj;

    while ( i < p.size() )
    {
        startObjIndex = i;
        //integration phase 1
        for ( ++i; i < p.size(); i++ )
            if ( fabs( p[i].getR() - p[i - 1].getR() ) >= 10)
                break;

        if( startObjIndex == i - 1 )
            continue;

        sObj = new LSfeatureObject( p , startObjIndex , i - 1 , ++s);

        //        if (( sObj->getLength() >= MaxLength ))
        //        {
        //            delete sObj;
        //            continue;
        //        }
        if (lst.size() > 0)
        {
            lastObj = lst.at( lst.size() - 1 );
            //integration phase 2
            if ( lastObj.getEndPoint().getDistanceFromPoint( sObj->getStartPoint() ) < 15)
            {
                vector<LSfeatureObject>::iterator ii = lst.begin();
                *sObj += lastObj;
                advance(ii,lst.size()-1);
                lst.erase(ii);
            }
        }
        lst.push_back(*sObj);
        delete sObj;
    }

    //------------------------------
    for(vector <LSfeatureObject>::iterator it=lst.begin();it != lst.end();)
    {
        double len = it->getScaledLength();
        double digonal = it->getDigonalBoundingBox();
        // if( (MinDgl <= digonal)&& (MaxDgl > digonal))

        if(round(len) == round(digonal))
        {
            it = lst.erase(it);
            continue;
        }
        if( (it->getWidth()<3))
        {
            it = lst.erase(it);
            continue;
        }
        if ( (len < MaxLength) && (len > minLength) && ((MinDgl <= digonal)&& (MaxDgl > digonal)))
        {
            //            cout<<"\n Slen : "<<len<<" len "<< it->getLength();
            //            cout<<"\n diagonal : "<< digonal;
            //            cout<<"\n width : "<<it->getWidth();
            //if( (it->getWidth()<=20))

            it->confidenceFactor = getStandardDeviation(digonal) ;
            it++;
        }
        else
        {
            it = lst.erase(it);
        }
    }
    return lst;
}

double featureObjectExtractor::getStandardDeviation(double x)
{
    double med = (MinDgl + MaxDgl) / 2;
    double delmed = MaxDgl - med;
    return exp(- fabs( x - delmed ) / med ) ;
}

void featureObjectExtractor::setDiagonal(double max, double min)
{
    MaxDgl = max;
    MinDgl = min;
}
